/*
 * Copyright 2015 Ettus Research LLC
 * Copyright 2018 Ettus Research, a National Instruments Company
 *
 * SPDX-License-Identifier: GPL-3.0-or-later
 * 丁劲犇修改 2021
 */

#include <stdio.h>
#include <stdlib.h>
#include <signal.h>
#include <string.h>
#include <string>
#include <thread>
#include <atomic>
#include <iostream>
#include <uhd.h>
#include <fftw3.h>
static bool stop_signal_called = false;
static char error_string[1024];
static char err_msg[1024];
#define UHD_DO(X) \
{\
	uhd_error e = (X);\
	if (e) { \
	uhd_error er = uhd_get_last_error(error_string,sizeof(error_string));\
	snprintf(err_msg,sizeof(err_msg),", line %d, Err %d.%d :",__LINE__,e,er);\
	std::string ev = __FILE__;\
	ev += err_msg;ev += error_string;\
	return_code = 1;\
	throw ev;\
	}\
	};
void sigint_handler(int code){
	(void)code;
	stop_signal_called = true;
}

int do_io(
		uhd_rx_streamer_handle rx,
		uhd_tx_streamer_handle tx,
		uhd_rx_metadata_handle rx_meta,
		uhd_tx_metadata_handle tx_meta);

typedef short SPTYPE;
int return_code = EXIT_SUCCESS;

static const int modraten = 4;
//Sample rate in Hz
const double sprate		=  250000 ;

int main(int /*argc*/, char* /*argv*/[])
{
	const char dev_args[] = "";
	//接收频率
	double rx_freq		= 200e6;
	double rx_sprate	= sprate;
	double rx_gain		= 55.0;
	bool   rx_agc		= true;
	double rx_bw		= sprate;
	//接收信号。MIMO时，可以指定0,1
	size_t rx_channel[]	= {0};
	const char rx_ana[] = "RX2";

	double tx_freq		= 200e6;
	double tx_sprate	= sprate;
	double tx_gain		= 70;
	double tx_bw		= sprate;
	//发射信号。MIMO时，可以指定0,1
	size_t tx_channel[]	= {1};
	const char tx_ana[] = "TX/RX";

	if (sizeof(rx_channel)>sizeof(rx_channel[0]) || sizeof(tx_channel)>sizeof(tx_channel[0]) )
	{
		fprintf(stderr,  "multi channels IO is not suitable for this simple demo.\n");
		return 0;
	}

	//设备句柄
	uhd_usrp_handle usrp = 0;
	uhd_rx_streamer_handle rx_streamer = 0;
	uhd_rx_metadata_handle rx_meta = 0;
	uhd_tx_streamer_handle tx_streamer = 0;
	uhd_tx_metadata_handle tx_meta = 0;



	try{
		fprintf(stderr, "Creating USRP with args \"%s\"...\n", dev_args);
		UHD_DO(uhd_usrp_make(&usrp, dev_args));

		puts("Press Enter to start...");
		// Ctrl+C will exit loop
		signal(SIGINT, &sigint_handler);
		fprintf(stderr, "Press Ctrl+C to stop streaming...\n");

		//设置天线，子板号为0（multi-usrp支持级联)
		UHD_DO(uhd_usrp_set_rx_antenna(usrp,rx_ana,rx_channel[0]));
		UHD_DO(uhd_usrp_set_tx_antenna(usrp,tx_ana,tx_channel[0]));
		// Create RX streamer
		UHD_DO(uhd_rx_streamer_make(&rx_streamer));
		// Create TX streamer
		UHD_DO(uhd_tx_streamer_make(&tx_streamer));
		// Create RX metadata
		UHD_DO(uhd_rx_metadata_make(&rx_meta));
		// Create TX metadata
		UHD_DO(uhd_tx_metadata_make(&tx_meta, false, 0, 0.1, true, false));
		// Create other necessary structs
		uhd_tune_request_t rx_tune_request =
		{
			.target_freq = rx_freq,
			.rf_freq_policy = UHD_TUNE_REQUEST_POLICY_AUTO,
			.rf_freq = 0,
			.dsp_freq_policy = UHD_TUNE_REQUEST_POLICY_AUTO,
			.dsp_freq = 0,
			.args = 0
		};
		uhd_tune_result_t rx_tune_result;
		//char rx_cpu_format[] = "fc32";
		char rx_cpu_format[] = "sc16";
		char rx_otw_format[] = "sc16";
		char rx_args[] = "";
		const size_t rx_channel_count = sizeof(rx_channel)/sizeof(rx_channel[0]);
		uhd_stream_args_t rx_stream_args = {
			.cpu_format = rx_cpu_format,
			.otw_format = rx_otw_format,
			.args = rx_args,
			.channel_list = rx_channel,
			.n_channels = rx_channel_count
		};

		// Create other necessary structs for TX
		uhd_tune_request_t tx_tune_request = {
			.target_freq = tx_freq,
			.rf_freq_policy = UHD_TUNE_REQUEST_POLICY_AUTO,
			.rf_freq = 0,
			.dsp_freq_policy = UHD_TUNE_REQUEST_POLICY_AUTO,
			.dsp_freq = 0,
			.args = 0
		};
		uhd_tune_result_t tx_tune_result;
		//char tx_cpu_format[] = "fc32";
		char tx_cpu_format[] = "sc16";
		char tx_otw_format[] = "sc16";
		char tx_args[] = "";
		const size_t tx_channel_count = sizeof(tx_channel)/sizeof(tx_channel[0]);

		uhd_stream_args_t tx_stream_args = {
			.cpu_format = tx_cpu_format,
			.otw_format = tx_otw_format,
			.args = tx_args,
			.channel_list = tx_channel,
			.n_channels = tx_channel_count
		};

		// Set rate
		fprintf(stderr, "Setting RX Rate: %f...\n", rx_sprate);
		UHD_DO(uhd_usrp_set_rx_rate(usrp, rx_sprate, rx_channel[0]));

		// See what rate actually is
		UHD_DO(uhd_usrp_get_rx_rate(usrp, rx_channel[0], &rx_sprate));
		fprintf(stderr, "Actual RX Rate: %f...\n", rx_sprate);

		// Set gain
		fprintf(stderr, "Setting RX Gain: %f dB...\n", rx_gain);
		UHD_DO(uhd_usrp_set_rx_gain(usrp, rx_gain, rx_channel[0], ""));

		// See what gain actually is
		UHD_DO(uhd_usrp_get_rx_gain(usrp, rx_channel[0], "", &rx_gain));
		fprintf(stderr, "Actual RX Gain: %f...\n", rx_gain);

		if (rx_agc)
		{
			uhd_usrp_set_rx_agc(usrp,true,rx_channel[0]);
			uhd_usrp_set_rx_dc_offset_enabled(usrp,true,rx_channel[0]);
		}
		// Set frequency
		fprintf(stderr, "Setting RX frequency: %f MHz...\n", rx_freq/1e6);
		UHD_DO(uhd_usrp_set_rx_freq(usrp, &rx_tune_request, rx_channel[0], &rx_tune_result));
		// See what frequency actually is
		UHD_DO(uhd_usrp_get_rx_freq(usrp, rx_channel[0], &rx_freq));
		fprintf(stderr, "Actual RX frequency: %f MHz...\n", rx_freq / 1e6);

		fprintf(stderr, "Setting RX Bandwidth: %f MHz...\n", rx_bw/1e6);
		UHD_DO(uhd_usrp_set_rx_bandwidth(usrp, rx_bw, rx_channel[0]));
		//Band
		UHD_DO(uhd_usrp_get_rx_bandwidth(usrp, rx_channel[0], &rx_bw));
		fprintf(stderr, "Actual RX Bandwidth: %f MHz...\n", rx_bw / 1e6);
		// Set rate
		fprintf(stderr, "Setting TX Rate: %f...\n", tx_sprate);
		UHD_DO(uhd_usrp_set_tx_rate(usrp, tx_sprate, tx_channel[0]));

		// See what rate actually is
		UHD_DO(uhd_usrp_get_tx_rate(usrp, tx_channel[0], &tx_sprate));
		fprintf(stderr, "Actual TX Rate: %f...\n\n", tx_sprate);

		// Set gain
		fprintf(stderr, "Setting TX Gain: %f db...\n", tx_gain);
		UHD_DO(uhd_usrp_set_tx_gain(usrp, tx_gain, tx_channel[0], ""));

		// See what gain actually is
		UHD_DO(uhd_usrp_get_tx_gain(usrp, tx_channel[0], "", &tx_gain));
		fprintf(stderr, "Actual TX Gain: %f...\n", tx_gain);

		// Set frequency
		fprintf(stderr, "Setting TX frequency: %f MHz...\n", tx_freq / 1e6);
		UHD_DO(uhd_usrp_set_tx_freq(usrp, &tx_tune_request, tx_channel[0], &tx_tune_result));

		// See what frequency actually is
		UHD_DO(uhd_usrp_get_tx_freq(usrp, tx_channel[0], &tx_freq));
		fprintf(stderr, "Actual TX frequency: %f MHz...\n", tx_freq / 1e6);

		//Band
		fprintf(stderr, "Setting TX Bandwidth: %f MHz...\n", tx_bw/1e6);
		UHD_DO(uhd_usrp_set_tx_bandwidth(usrp, tx_bw,tx_channel[0]));

		UHD_DO(uhd_usrp_get_tx_bandwidth(usrp, tx_channel[0], &tx_bw));
		fprintf(stderr, "Actual TX Bandwidth: %f MHz...\n", tx_bw / 1e6);


		// Set up streamer
		rx_stream_args.channel_list = rx_channel;
		UHD_DO(uhd_usrp_get_rx_stream(usrp, &rx_stream_args, rx_streamer));

		// Set up streamer
		tx_stream_args.channel_list = tx_channel;
		UHD_DO(uhd_usrp_get_tx_stream(usrp, &tx_stream_args, tx_streamer));


		do_io(rx_streamer,tx_streamer,rx_meta,tx_meta);

	}
	catch(std::string s)
	{
		fputs(s.c_str(),stderr);
	}




	if (tx_streamer) uhd_tx_streamer_free(&tx_streamer);
	if (rx_streamer) uhd_rx_streamer_free(&rx_streamer);
	if (tx_meta) uhd_tx_metadata_free(&tx_meta);
	if (rx_meta) uhd_rx_metadata_free(&rx_meta);

	if(return_code != EXIT_SUCCESS && usrp != NULL){
		uhd_usrp_last_error(usrp, error_string, 512);
		fprintf(stderr, "USRP reported the following error: %s\n", error_string);
	}
	uhd_usrp_free(&usrp);

	fprintf(stderr, (return_code ? "Failure\n" : "Success\n"));
	return return_code;
}


//收发计数
static std::atomic< unsigned long long> rx_count (0), tx_count (0);
//消费生产计数
static std::atomic< unsigned long long> deal_count (0), gene_count (0);
//缓存点数(IQ)
static const size_t rxbuf_points = 1000*1000*16;
static const size_t txbuf_points = 1000*1000*16;
//缓存,留有尾巴保护
static std::shared_ptr<short [][2]> rx_buf_ptr(new short[rxbuf_points+1024*1024][2]{{0,0}});
static std::shared_ptr<short [][2]> tx_buf_ptr(new short[txbuf_points+1024*1024][2]{{0,0}});
void init_wavform();

//生产者线程,for tx
void producer();

//消费者线程，for rx
void dealer();

int do_io(
		uhd_rx_streamer_handle rx_streamer,
		uhd_tx_streamer_handle tx_streamer,
		uhd_rx_metadata_handle rx_meta,
		uhd_tx_metadata_handle tx_meta)
{

	uhd_stream_cmd_t rx_stream_cmd = {
		.stream_mode = UHD_STREAM_MODE_START_CONTINUOUS,
		.num_samps = 0,
		.stream_now = true,
		.time_spec_full_secs = 0,
		.time_spec_frac_secs = 0
	};
	size_t rx_sps_buff = 0;
	size_t tx_sps_buff = 0;
	// Set up buffer
	UHD_DO(uhd_rx_streamer_max_num_samps(rx_streamer, &rx_sps_buff));
	fprintf(stderr, "Buffer size in samples: %zu\n", rx_sps_buff);
	// Set up buffer
	UHD_DO(uhd_tx_streamer_max_num_samps(tx_streamer, &tx_sps_buff));
	fprintf(stderr, "Buffer size in samples: %zu\n", tx_sps_buff);
	//初始化队列
	short (*pBufRx)[2] = rx_buf_ptr.get();
	short (*pBufTx)[2] = tx_buf_ptr.get();
	rx_count = 0;
	tx_count = 0;
	deal_count = 0;
	gene_count = 0;
	using std::thread;
	//接收线程
	auto thcall_rx = [&]()->void{
		// Issue stream command
		fprintf(stderr, "Issuing stream command.\n");
		UHD_DO(uhd_rx_streamer_issue_stream_cmd(rx_streamer, &rx_stream_cmd));
		while (!stop_signal_called)
		{
			size_t red = 0;
			void * rx_buff_ptr = (void *)(pBufRx[rx_count % rxbuf_points]);
			UHD_DO(uhd_rx_streamer_recv(rx_streamer,&rx_buff_ptr, rx_sps_buff, &rx_meta, 1, false, &red));
			rx_count += red;
			const size_t newBg = rx_count % rxbuf_points;
			if (newBg < rx_sps_buff &&  newBg > 0)
				memcpy(pBufRx[0], pBufRx[rxbuf_points], sizeof(short) * 2 * newBg);
			uhd_rx_metadata_error_code_t error_code;
			UHD_DO(uhd_rx_metadata_error_code(rx_meta, &error_code));
			if(error_code != UHD_RX_METADATA_ERROR_CODE_NONE){
				fprintf(stderr, "Warning: Error code 0x%x was returned during streaming.\n", error_code);
			}
		}
	};
	//发射线程
	auto thcall_tx = [&]()->void{
		//uhd::tx_metadata_t md_tx;
		//md_tx.end_of_burst   = false;
		//md_tx.start_of_burst = false;
		while (!stop_signal_called)
		{
			const size_t cur_bg = tx_count % txbuf_points;
			const size_t new_bg = (tx_count + tx_sps_buff) % txbuf_points;
			if (new_bg < tx_sps_buff && new_bg > 0)
				memcpy(pBufTx[txbuf_points],pBufTx[0],sizeof(short) * 2 * new_bg);
			const void * tx_buff_ptr = (const void *)(pBufTx[cur_bg]);
			size_t sent = 0;
			UHD_DO(uhd_tx_streamer_send(tx_streamer, &tx_buff_ptr, tx_sps_buff, &tx_meta, 1, &sent));
			tx_count += sent;
		}
	};

	init_wavform();

	//启动线程
	thread produce_thread(producer);
	thread tx_thread(thcall_tx);
	thread rx_thread(thcall_rx);
	thread deal_thread(dealer);

	//主线程不断打印状态
	while (!stop_signal_called)
	{
		std::cerr<<"\nProduce " << gene_count<<" TX "<< tx_count<< " RX "<< rx_count << " Deal "<< deal_count<<
			  " Delay = "<<rx_count - deal_count<<
			  "    \n";
		std::this_thread::sleep_for(std::chrono::milliseconds(1000));

		if (rx_count >= 4*1024*1024)
			stop_signal_called = true;
	}
	//退出
	produce_thread.join();
	rx_thread.join();
	tx_thread.join();
	deal_thread.join();
}


static const int amp = 8192;
static const int spread_ratio = 128;
static const size_t wav_size = spread_ratio*modraten;
static short wav_spread[2][wav_size][2];
static float wav_xorr[2][wav_size][2];



void init_wavform()
{
	const double pi = 3.14159265358979323846;
	// 2FSK, 0/1,  modraten samples, i,q
	short wav[2][modraten][2];
	for (int i=0;i<modraten;++i)
	{
		wav[0][i][0] = amp ;
		wav[0][i][1] = amp ;
		wav[1][i][0] = -amp ;
		wav[1][i][1] = -amp ;
	}
	//扩频
	char reg [21] = {0,1,1,0,0,1,0,1,1,0,0,1,0,1,1,0,0,1,0,1,1};
	//seq[0]是 0的符号，1是1的符号。每个符号中的0,1又对应了wav的IQ路
	char seq[2][spread_ratio];
	int sw = 0;
	//产生0,1的波形
	fprintf (stderr,"0=");

	for (int i=0;i<spread_ratio;++i)
	{
		reg[sw % 21] = reg[(sw + 3) % 21] ^  reg[(sw+20) % 21];
		seq[0][i] = reg[sw % 21];
		fprintf (stderr,"%d",seq[0][i]);
		--sw;
		if (sw < 0)
			sw = 20;
		for (int j=0;j<modraten;++j)
		{
			wav_spread[0][i*modraten+j][0] = wav[seq[0][i]][j][0];
			wav_spread[0][i*modraten+j][1] = wav[seq[0][i]][j][1];
			wav_xorr[0][i*modraten+j][0] = wav[seq[0][i]][j][0]*1.0/amp;
			wav_xorr[0][i*modraten+j][1] = -wav[seq[0][i]][j][1]*1.0/amp;
		}
	}
	for (int i=0;i<377;++i)
	{
		reg[sw % 21] = reg[(sw + 3) % 21] ^  reg[(sw+20) % 21];
		--sw;
		if (sw < 0)
			sw = 20;
	}
	fprintf (stderr,"\n1=");

	for (int i=0;i<spread_ratio;++i)
	{
		reg[sw % 21] = reg[(sw + 3) % 21] ^  reg[(sw+20) % 21];
		seq[1][i] = reg[sw % 21];
		fprintf (stderr,"%d",seq[1][i]);
		--sw;
		if (sw < 0)
			sw = 20;
		for (int j=0;j<modraten;++j)
		{
			wav_spread[1][i*modraten+j][0] = wav[seq[1][i]][j][0];
			wav_spread[1][i*modraten+j][1] = wav[seq[1][i]][j][1];
			wav_xorr[1][i*modraten+j][0] = wav[seq[1][i]][j][0]*1.0/amp;
			wav_xorr[1][i*modraten+j][1] = -wav[seq[1][i]][j][1]*1.0/amp;
		}
	}

	fprintf(stderr, "\nData inited.\n");

}

//生产者线程,for tx
void producer()
{

	short (*pBufTx)[2] = tx_buf_ptr.get();

	//Push 0x7e
	char idle[8] = {0,1,1,1,1,1,1,0};
	while (!stop_signal_called)
	{
		if (gene_count > tx_count + txbuf_points-16)
		{
			std::this_thread::sleep_for(std::chrono::milliseconds(1));
			continue;
		}
		const unsigned long long clk_wav_sprd = gene_count % wav_size;
		const unsigned long long clk_bits     = (gene_count / wav_size) % 8;
		const int curr_bit = idle[clk_bits];
		pBufTx[gene_count % txbuf_points][0] = wav_spread[curr_bit][clk_wav_sprd][0];
		pBufTx[gene_count % txbuf_points][1] = wav_spread[curr_bit][clk_wav_sprd][1];
		++gene_count;
	}

}

//消费者线程，for rx
void dealer()
{
	unsigned long long next_test = wav_size;
	const unsigned int half_sz = wav_size/2;
	short (*pBufRx)[2] = rx_buf_ptr.get();
	double (* ampwin)[2] = new double[wav_size][2];
	
	fftw_complex *in[2], *out[2];
	fftw_plan p[2];
    
	in[0] = (fftw_complex*) fftw_malloc(sizeof(fftw_complex) * wav_size);
	out[0] = (fftw_complex*) fftw_malloc(sizeof(fftw_complex) * wav_size);
	p[0] = fftw_plan_dft_1d(wav_size, in[0], out[0], FFTW_FORWARD, FFTW_ESTIMATE);
	in[1] = (fftw_complex*) fftw_malloc(sizeof(fftw_complex) * wav_size);
	out[1] = (fftw_complex*) fftw_malloc(sizeof(fftw_complex) * wav_size);
	p[1] = fftw_plan_dft_1d(wav_size, in[1], out[1], FFTW_FORWARD, FFTW_ESTIMATE);
	//fftw_execute(p); /* repeat as needed */
	double globalMax = 0;
	while (!stop_signal_called)
	{
		if (deal_count + wav_size >= rx_count || rx_count < wav_size )
		{
			std::this_thread::sleep_for(std::chrono::milliseconds(1));
			continue;
		}

		if (deal_count < next_test)
		{
			ampwin[deal_count % wav_size][0] = 0;
			ampwin[deal_count % wav_size][1] = 0;
			++deal_count;
			continue;
		}
		unsigned long long start_x = deal_count - wav_size;
		//xorr
#pragma omp simd
		for (size_t i=0;i<wav_size;++i)
		{
			const int xb = (start_x + i) % rxbuf_points;
			in[0][i][0] = (pBufRx[xb][0] * wav_xorr[0][i][0] - pBufRx[xb][1] * wav_xorr[0][i][1])/16384.0;
			in[0][i][1] = (pBufRx[xb][0] * wav_xorr[0][i][1] + pBufRx[xb][1] * wav_xorr[0][i][0])/16384.0;
			in[1][i][0] = (pBufRx[xb][0] * wav_xorr[1][i][0] - pBufRx[xb][1] * wav_xorr[1][i][1])/16384.0;
			in[1][i][1] = (pBufRx[xb][0] * wav_xorr[1][i][1] + pBufRx[xb][1] * wav_xorr[1][i][0])/16384.0;
		}
		fftw_execute(p[0]);
		fftw_execute(p[1]);
		double max_abs0 = 0, max_abs1 = 0;
		for (size_t i=0;i<wav_size;++i)
		{
			const double a0 = out[0][i][0]*out[0][i][0] + out[0][i][1]*out[0][i][1];
			const double a1 = out[1][i][0]*out[1][i][0] + out[1][i][1]*out[1][i][1];
			if (a0 > max_abs0)
				max_abs0 = a0;
			if (a1 > max_abs1)
				max_abs1 = a1;
		}

		ampwin[deal_count % wav_size][0] = max_abs0;
		ampwin[deal_count % wav_size][1] = max_abs1;

		if (globalMax  < max_abs0)
		{
			printf("%lf,%lf\n",max_abs0,max_abs1);
			globalMax = max_abs0;
		}
		if (globalMax  < max_abs1)
		{
			printf("%lf,%lf\n",max_abs0,max_abs1);
			globalMax = max_abs1;
		}


		bool best0 = true, best1=true;

		for (size_t i=0;i<wav_size && (best0||best1);++i)
		{
			if (i!=half_sz)
			{
				if (ampwin[(deal_count +i - wav_size  )% wav_size][0] >= ampwin[(deal_count +half_sz - wav_size  )% wav_size][0])
					best0 = false;
				if (ampwin[(deal_count +i - wav_size  )% wav_size][1] >= ampwin[(deal_count +half_sz - wav_size  )% wav_size][1])
					best1 = false;
			}
		}
		if (ampwin[(deal_count +half_sz - wav_size  )% wav_size][1]*10 >= ampwin[(deal_count +half_sz - wav_size  )% wav_size][0])
			best0 = false;
		if (ampwin[(deal_count +half_sz - wav_size  )% wav_size][0]*10 >= ampwin[(deal_count +half_sz - wav_size  )% wav_size][1])
			best1 = false;

		if (best0  )
		{
			next_test = deal_count + wav_size/2 - modraten * 16;
			putchar('0');
		}
		else if (best1 )
		{
			next_test = deal_count + wav_size/2 - modraten * 16;
			putchar('1');
		}
		++deal_count;
	}
	delete [] ampwin;
	fftw_destroy_plan(p[0]);
	fftw_free(in[0]); fftw_free(out[0]);
	fftw_destroy_plan(p[1]);
	fftw_free(in[1]); fftw_free(out[1]);

}
